#include "../../include/api/gnssApi.h"
#include "../../include/api/dtuApi.h"
#include "../../include/driver/driver_usart.h"
#include "../../include/device/rtkDevice.h"

/* ecal 和 protocol */
#include "control.pb.h"
#include "localization.pb.h"
#include <google/protobuf/io/zero_copy_stream_impl.h>
#include <google/protobuf/text_format.h>
#include <ctype.h>
#include <ecal/ecal.h>
#include <ecal/msg/protobuf/publisher.h>
#include <ecal/msg/protobuf/subscriber.h>
/*******************************************************************
 * @author: tianxiaohuahua
 * @date: 23.8
 * @description: C_gnssApi 构造函数 用于对类的成员变量初始化赋值
 * @input: void
 * @return: NULL
*******************************************************************/
C_gnssApi::C_gnssApi(/* args */)
{
    p_rtkDevice = NULL;
    m_gnssRecvBuf = 0;
    m_gnssRecvBuf = (GnssRecvBuf*)malloc(sizeof(GnssRecvBuf));
    spdlog::info("C_gnssApi, init");
}


/********************************************************************
 * @author: tianxiaohuahua
 * @date: 23.8
 * @description: C_gnssApi 析构函数 用于对类清理空间
 * @input: void
 * @return: NULL
*******************************************************************/
C_gnssApi::~C_gnssApi()
{
    delete p_rtkDevice;
    p_rtkDevice = NULL;
}

/********************************************************************
 * @author: tianxiaohuahua
 * @date: 23.8
 * @description: initTask 基于基类虚函数集成方法，用于统一对参数初始化
 * @input: CONFIG_GNSS gnss的模块需要的硬件和线程定义信息
 * @return: int32 返回成功失败
*******************************************************************/
int32_t C_gnssApi::initTask(CONFIG_GNSS &config_gnss)
{
    spdlog::info("..............................................................");
    spdlog::info("C_gnssApi::initTask");
    task_name   = config_gnss.config_task.task_name;
    task_period = config_gnss.config_task.task_period;
    task_topic  = config_gnss.config_task.task_topic;
    spdlog::info("C_gnssApi task_name:'{}'", task_name);
    spdlog::info("C_gnssApi task_period:'{}'", task_period);
    spdlog::info("C_gnssApi task_topic:'{}'", task_topic);
    spdlog::info("C_gnssApi nband:          '{}'",config_gnss.config_serial_hardware.baud);
    spdlog::info("C_gnssApi ndata_bits:     '{}'",config_gnss.config_serial_hardware.data_bits);
    spdlog::info("C_gnssApi nserial_device: '{}'",config_gnss.config_serial_hardware.serial_device );
    spdlog::info("C_gnssApi nstop_bits:     '{}'",config_gnss.config_serial_hardware.stop_bits);
    spdlog::info("..............................................................");
    p_rtkDevice = new C_rtkDevice(config_gnss.config_serial_hardware.serial_device.c_str(), config_gnss.config_serial_hardware.baud);
    if(p_rtkDevice == NULL){
        return false;
    }
    return true;
}


/********************************************************************
 * @author: tianxiaohuahua
 * @date: 23.8
 * @description: initTask 基于基类虚函数集成方法，用于统一对参数初始化
 * @input: CONFIG_GNSS gnss的模块需要的硬件和线程定义信息
 * @return: int32 返回成功失败
*******************************************************************/
void C_gnssApi::runLoopTask(void)
{
    // 新建一个线程用于转发RTCM数据到串口
    std::thread thread = std::thread(&C_gnssApi::transmitRtcm, this); // 只能这么写，不这么写会报错
    eCAL::protobuf::CPublisher<MowerProtoPakg::localizationGNGGA> ecalGNGGApublisher("GNGGAMessage");
    std::vector<uint8_t> gnssBufStr;
    S_GNGGA gnggaBuf; // 存储GNGGA语句的结构体
    int32_t recv = 0;
    while(1){
        recv = recvGnssData(gnssBufStr);// 接收串口的GNSS定位数据
        if(true == recv){ 
            recv = parseGnssData(gnssBufStr, gnggaBuf); // 解析GNSS定位数据，找到不同的定位语句
            if(true == recv){
                // 把数据包装成protobuf
                MowerProtoPakg::localizationGNGGA localizationGNGGAMessage;// 创建一个protobuf消息对象
                localizationGNGGAMessage.set_time(gnggaBuf.time);
                localizationGNGGAMessage.set_latitude(gnggaBuf.latitude);
                localizationGNGGAMessage.set_latituderange(gnggaBuf.latitudeRange);
                localizationGNGGAMessage.set_longitude(gnggaBuf.longitude);
                localizationGNGGAMessage.set_longituderange(gnggaBuf.longitudeRange);
                localizationGNGGAMessage.set_gpsstatus(gnggaBuf.gpsStatus);
                localizationGNGGAMessage.set_startnum(gnggaBuf.startNum);
                localizationGNGGAMessage.set_precision(gnggaBuf.precision);
                localizationGNGGAMessage.set_altitude(gnggaBuf.altitude);
                localizationGNGGAMessage.set_geoidheight(gnggaBuf.geoidHeight);
                // Send the message
                ecalGNGGApublisher.Send(localizationGNGGAMessage);
                // spdlog::info("C_gnssApi, Sent proto message!");
                //std::this_thread::sleep_for(std::chrono::milliseconds(1000));
            
                // 如果解析成功，把经纬数据转换成protbuf数据
                // 发布protubuf数据
            }
            else{
                // spdlog::info("recv gnss data is wrong!");
            }
        }         
    }
    thread.join();
}


/********************************************************************
 * @author: tianxiaohuahua
 * @date: 23.8
 * @description: recvRTCMdata 接收RTCM数据
 * @input: void
 * @return: int32 返回成功状态
*******************************************************************/
int32_t C_gnssApi::recvRTCMdata(void)
{
    int32_t recv = 0;
    recv = getRTCMdata(m_rtcmData.rtcmData, 512);
    if(recv == SAND_RTCM_DATA_ERROR){
        spdlog::info("input error\r\n"); // 数据错误
    } 
    else if(recv == SAND_RTCM_DATA_WAIT){
        //printf(".\r\n"); // 等待RTCM数据
    }
    else{
        m_rtcmData.rtcmLen = recv;
        return true;
    }
    return false;
}


/********************************************************************
 * @author: tianxiaohuahua
 * @date: 23.8
 * @description: initTask 一个线程用于转发RTCM数据到串口
 * @input: void 
 * @return: void 
*******************************************************************/
void C_gnssApi::transmitRtcm(void)
{
    int32_t recv = 0;
    while(1){
        recv = recvRTCMdata();     // 接收RTCM数据 
        if(recv ==  true){
            p_rtkDevice->sandUsartData(m_rtcmData.rtcmData, m_rtcmData.rtcmLen);// 转发RTCM数据
        }
    } 
}


/********************************************************************
 * @author: tianxiaohuahua
 * @date: 23.8
 * @description: recvGnssData 接收gnss定位数据
 * @input: void
 * @return: void
*******************************************************************/
int32_t C_gnssApi::recvGnssData(std::vector<uint8_t> &gnssBufStr)
{
    uint32_t recv = 0;
    recv = p_rtkDevice->recvUsartData(p_rtkDevice->m_gnssStrBuffer); // 如果采集到了串口数据字符串
    if(recv == RECV_DONE){
        gnssBufStr =  p_rtkDevice->m_gnssStrBuffer; // 将数据传递到储存buf中
        return true;
        //printf("recv:%d\n%s\r\n", recv, m_gnssRecvBuf.recvBuf);
    }
    else{
        return false;
    }
}


/********************************************************************
 * @author: tianxiaohuahua
 * @date: 23.8
 * @description: 字符串分割函数
 * @reference: https://blog.csdn.net/birenxiaofeigg/article/details/115464934
 * @input: void
 * @return: void
*******************************************************************/
std::vector<std::string> split(std::string str, std::string pattern)
{
    std::string::size_type pos;
    std::vector<std::string> result;
    str += pattern;//扩展字符串以方便操作
    int size = str.size();
    for (int i = 0; i < size; i++)
    {
        pos = str.find(pattern, i);
        if (pos < size)
        {
            std::string s = str.substr(i, pos - i);
            result.push_back(s);
            i = pos + pattern.size() - 1;
        }
    }
    return result;
}


/********************************************************************
 * @author: tianxiaohuahua
 * @date: 23.8
 * @description: 判断输入的字符串是不是全是数字，整数或者小数的形式
 * @input: 字符串
 * @return: 是否是数字
*******************************************************************/
bool judgeStrToInt(std::string &str)// 判断字符串
{
    for(uint32_t i=0; i<str.length(); i++){
        if(std::isdigit(str[i]) == 0){ // 当判断的字符不是数字时，函数返回 0
            if(str[i] != '.'){ // 如果不是数字也不是小数点的时候
                return false;
            }
        }
    }
    return true;
}

/********************************************************************
 * @author: tianxiaohuahua
 * @date: 23.8
 * @description: 将接收到的GNSS定位数据转换为经纬度数据
 * @input: data 直接获取到的经纬度的数据 四位数的小数
 * @return: data_ 转换后的数据
*******************************************************************/
double changeData(double data)
{
    double data_ = (int)(data / 100) + fmod(data, 100) / 60.0;  // 根据手册将GNGGA数据转换为经纬度数据
    return data_;
}

int32_t C_gnssApi::parseGnssData(std::string &str, S_GNGGA &gnggaData)
{
    uint32_t index = str.find("GNGGA", 0); // 在srt字符串中寻找“GNGGA”字符串的位置
    if(index < str.length()){ // 如果找到了GNGGA语句
        std::vector<std::string> result=split(str,","); // 按照逗号对GNGGA语句进行分割
        // std::cout<<"The result:"<<std::endl; // 输出分割结果
        for(int i=0; i<result.size(); i++) {
            //std::cout<<result[i]<<std::endl;
            switch (i)
            {
            case 1: // 字段1：UTC 时间，hhmmss.sss，时分秒格式
                if(judgeStrToInt(result[i]) == true){ // 如果字符串全是数字
                    gnggaData.time = stod(result[i],0); // 把字符串s从p开始转换成double
                    // printf("字段1 UTC 时间:%f\n", gnggaData.time);
                }                
                break;
            case 2: // 字段2：纬度ddmm.mmmm，度分格式（前导位数不足则补0）
                if(judgeStrToInt(result[i]) == true){ // 如果字符串全是数字
                    gnggaData.latitude =  changeData(stod(result[i],0)); // 把字符串s从p开始转换成double // 再转换成经纬度
                    // printf("字段2 纬度:%lf\n", gnggaData.latitude);
                }   
                break;
            case 3: // 字段3：纬度N（北纬）或S（南纬）
                {
                    gnggaData.latitudeRange = result[i]; // 把字符串s从p开始转换成double
                    // std::cout << "字段3 纬度:" << gnggaData.latitudeRange << std::endl;
                }   
                break;
            case 4: // 字段4：经度dddmm.mmmm，度分格式（前导位数不足则补0）
                if(judgeStrToInt(result[i]) == true){ // 如果字符串全是数字
                    gnggaData.longitude = changeData(stod(result[i],0)); // 把字符串s从p开始转换成double // 再转换成经纬度
                    // printf("字段4 经度:%lf\n", gnggaData.longitude);
                }   
                break;
            case 5: // 字段5：经度E（东经）或W（西经）
                {
                    gnggaData.longitudeRange = result[i]; // 把字符串s从p开始转换成double
                    // std::cout << "字段5 经度:" << gnggaData.longitudeRange << std::endl;
                }   
                break;
            case 6: // 字段6：GPS状态，0=未定位，1=非差分定位，2=差分定位，3=无效PPS，6=正在估算
                if(judgeStrToInt(result[i]) == true){ // 如果字符串全是数字
                    gnggaData.gpsStatus = stod(result[i],0); // 把字符串s从p开始转换成double
                    // std::cout << "字段6 GPS状态:" << gnggaData.gpsStatus << std::endl;
                }   
                break;
            case 7: // 字段7：正在使用的卫星数量（00 - 12）（前导位数不足则补0）
                if(judgeStrToInt(result[i]) == true){ // 如果字符串全是数字
                    gnggaData.startNum = stod(result[i],0); // 把字符串s从p开始转换成double
                    // std::cout << "字段7 卫星数量:" << gnggaData.startNum << std::endl;
                }   
                break;
            default:{}
                break;
            }
        }
        switch (gnggaData.gpsStatus) // 字段6：GPS状态，0=未定位，1=非差分定位，2=差分定位，3=无效PPS，6=正在估算
        {
        case 0:
            spdlog::info("C_gnssApi UTC:{} 纬度:{}{} 经度:{}{} GPS状态:未定位 卫星数量:{}",
            gnggaData.time, gnggaData.latitude, gnggaData.latitudeRange, gnggaData.longitude, gnggaData.longitudeRange, gnggaData.startNum);
            break;
        case 1:
            spdlog::info("C_gnssApi UTC:{} 纬度:{}{} 经度:{}{} GPS状态:非差分定位 卫星数量:{}",
            gnggaData.time, gnggaData.latitude, gnggaData.latitudeRange, gnggaData.longitude, gnggaData.longitudeRange, gnggaData.startNum);
            break;
        case 2:
            spdlog::info("C_gnssApi UTC:{} 纬度:{}{} 经度:{}{} GPS状态:差分定位 卫星数量:{}",
            gnggaData.time, gnggaData.latitude, gnggaData.latitudeRange, gnggaData.longitude, gnggaData.longitudeRange, gnggaData.startNum);
            break;
        case 3:
            spdlog::info("C_gnssApi UTC:{} 纬度:{}{} 经度:{}{} GPS状态:无效PPS 卫星数量:{}",
            gnggaData.time, gnggaData.latitude, gnggaData.latitudeRange, gnggaData.longitude, gnggaData.longitudeRange, gnggaData.startNum);
            break;
        case 6:
            spdlog::info("C_gnssApi UTC:{} 纬度:{}{} 经度:{}{} GPS状态:正在估算 卫星数量:{}",
            gnggaData.time, gnggaData.latitude, gnggaData.latitudeRange, gnggaData.longitude, gnggaData.longitudeRange, gnggaData.startNum);
            break;
        
        default:
            break;
        }
        
        return true;
    }
    return false;
    // printf("recv:\n");
    // uint8_t* pos_gnss_buffer = p_rtkDevice->m_gnssStrBuffer.data(); // 打印
    // for (int i = 0; i < p_rtkDevice->m_gnssStrBuffer.size(); i++){
    //     printf("%c",*pos_gnss_buffer++);
    // } //printf("\n");
    
}

/********************************************************************
 * @author: tianxiaohuahua
 * @date: 23.8
 * @description: 将接收到的GNSS定位语句等解析成为经纬高数据
 * @input: void
 * @return: void
*******************************************************************/
int32_t C_gnssApi::parseGnssData(std::vector<uint8_t> &gnssBufStr, S_GNGGA &gnggaData)
{
    std::string str(gnssBufStr.begin(), gnssBufStr.end());
    uint32_t index = str.find("GNGGA", 0); // 在srt字符串中寻找“GNGGA”字符串的位置
    if(index < str.length()){ // 如果找到了GNGGA语句
        std::vector<std::string> result=split(str,","); // 按照逗号对GNGGA语句进行分割
        // std::cout<<"The result:"<<std::endl; // 输出分割结果
        for(int i=0; i<result.size(); i++) {
            //std::cout<<result[i]<<std::endl;
            switch (i)
            {
            case 1: // 字段1：UTC 时间，hhmmss.sss，时分秒格式
                if(judgeStrToInt(result[i]) == true){ // 如果字符串全是数字
                    gnggaData.time = stod(result[i],0); // 把字符串s从p开始转换成double
                    // printf("字段1 UTC 时间:%f\n", gnggaData.time);
                }                
                break;
            case 2: // 字段2：纬度ddmm.mmmm，度分格式（前导位数不足则补0）
                if(judgeStrToInt(result[i]) == true){ // 如果字符串全是数字
                    gnggaData.latitude =  changeData(stod(result[i],0)); // 把字符串s从p开始转换成double // 再转换成经纬度
                    // printf("字段2 纬度:%lf\n", gnggaData.latitude);
                }   
                break;
            case 3: // 字段3：纬度N（北纬）或S（南纬）
                {
                    gnggaData.latitudeRange = result[i]; // 把字符串s从p开始转换成double
                    // std::cout << "字段3 纬度:" << gnggaData.latitudeRange << std::endl;
                }   
                break;
            case 4: // 字段4：经度dddmm.mmmm，度分格式（前导位数不足则补0）
                if(judgeStrToInt(result[i]) == true){ // 如果字符串全是数字
                    gnggaData.longitude = changeData(stod(result[i],0)); // 把字符串s从p开始转换成double // 再转换成经纬度
                    // printf("字段4 经度:%lf\n", gnggaData.longitude);
                }   
                break;
            case 5: // 字段5：经度E（东经）或W（西经）
                {
                    gnggaData.longitudeRange = result[i]; // 把字符串s从p开始转换成double
                    // std::cout << "字段5 经度:" << gnggaData.longitudeRange << std::endl;
                }   
                break;
            case 6: // 字段6：GPS状态，0=未定位，1=非差分定位，2=差分定位，3=无效PPS，6=正在估算
                if(judgeStrToInt(result[i]) == true){ // 如果字符串全是数字
                    gnggaData.gpsStatus = stod(result[i],0); // 把字符串s从p开始转换成double
                    // std::cout << "字段6 GPS状态:" << gnggaData.gpsStatus << std::endl;
                }   
                break;
            case 7: // 字段7：正在使用的卫星数量（00 - 12）（前导位数不足则补0）
                if(judgeStrToInt(result[i]) == true){ // 如果字符串全是数字
                    gnggaData.startNum = stod(result[i],0); // 把字符串s从p开始转换成double
                    // std::cout << "字段7 卫星数量:" << gnggaData.startNum << std::endl;
                }   
                break;
            default:{}
                break;
            }
        }
        switch (gnggaData.gpsStatus) // 字段6：GPS状态，0=未定位，1=非差分定位，2=差分定位，3=无效PPS，6=正在估算
        {
        case 0:
            spdlog::info("C_gnssApi UTC:{} 纬度:{}{} 经度:{}{} GPS状态:未定位 卫星数量:{}",
            gnggaData.time, gnggaData.latitude, gnggaData.latitudeRange, gnggaData.longitude, gnggaData.longitudeRange, gnggaData.startNum);
            break;
        case 1:
            spdlog::info("C_gnssApi UTC:{} 纬度:{}{} 经度:{}{} GPS状态:非差分定位 卫星数量:{}",
            gnggaData.time, gnggaData.latitude, gnggaData.latitudeRange, gnggaData.longitude, gnggaData.longitudeRange, gnggaData.startNum);
            break;
        case 2:
            spdlog::info("C_gnssApi UTC:{} 纬度:{}{} 经度:{}{} GPS状态:差分定位 卫星数量:{}",
            gnggaData.time, gnggaData.latitude, gnggaData.latitudeRange, gnggaData.longitude, gnggaData.longitudeRange, gnggaData.startNum);
            break;
        case 3:
            spdlog::info("C_gnssApi UTC:{} 纬度:{}{} 经度:{}{} GPS状态:无效PPS 卫星数量:{}",
            gnggaData.time, gnggaData.latitude, gnggaData.latitudeRange, gnggaData.longitude, gnggaData.longitudeRange, gnggaData.startNum);
            break;
        case 6:
            spdlog::info("C_gnssApi UTC:{} 纬度:{}{} 经度:{}{} GPS状态:正在估算 卫星数量:{}",
            gnggaData.time, gnggaData.latitude, gnggaData.latitudeRange, gnggaData.longitude, gnggaData.longitudeRange, gnggaData.startNum);
            break;
        
        default:
            break;
        }
        
        return true;
    }
    return false;
    // printf("recv:\n");
    // uint8_t* pos_gnss_buffer = p_rtkDevice->m_gnssStrBuffer.data(); // 打印
    // for (int i = 0; i < p_rtkDevice->m_gnssStrBuffer.size(); i++){
    //     printf("%c",*pos_gnss_buffer++);
    // } //printf("\n");
}